/*
Robotics object oriented package in C++
Copyright (C) 2008-2009  Matrix

This library is free software; you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as
published by the Free Software Foundation; either version 2.1 of the
License, or (at your option) any later version.

This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General Public
License along with this library; if not, write to the Free Software
Foundation, Inc., 59 Temple Place, Suite 330, Boston,  MA 02111-1307  USA
*/

#include "StdAfx.h"
#include "Matrix.h"

namespace Robotic { namespace Math{
	Matrix::Matrix()
	{
		_row = 0;
		_column = 0;
		_datas = NULL;
	}
	Matrix::Matrix(const int row, const int col)
	{
		_row = row;
		_column = col;
		_datas = new double[_row * _column];
	}

	Matrix::Matrix(const Matrix& arg)
	{
		_row = arg._row;
		_column = arg._column;
		int length = _row * _column;
		_datas = new double[length];

		for(int i=0; i< length; i++)
		{
			_datas[i] = arg._datas[i];
		}
	}

	Matrix::Matrix(const ColumnVector& arg)
	{
		_row = arg.GetCount();
		_column = 1;
		_datas = new double[_row];

		for ( int i = 0; i < _row; i++)
		{
			(*this)(i, 0) = arg[i];
		}
	}

	//Matrix::Matrix(const Vector2& arg)
	//{
	//	_row = 2;
	//	_column = 1;
	//	_datas = new double[_row];

	//	for ( int i = 0; i < _row; i++)
	//	{
	//		(*this)(i, 0) = arg[i];
	//	}
	//}

	//Matrix::Matrix(const Vector3& arg)
	//{
	//	_row = 3;
	//	_column = 1;
	//	_datas = new double[_row];

	//	for ( int i = 0; i < _row; i++)
	//	{
	//		(*this)(i, 0) = arg[i];
	//	}
	//}

	Matrix::Matrix(const Rotation2& arg)
	{
		_row = 2;
		_column = 2;
		int length = _row * _column;
		_datas = new double[length];

		for ( int i = 0; i < _row; i++)
		{
			for ( int j = 0; j < _column; j++) 
			{
				(*this)(i, j) = arg(i, j);
			}
		}
	}

	Matrix::Matrix(const Rotation3& arg)
	{
		_row = 3;
		_column = 3;
		int length = _row * _column;
		_datas = new double[length];

		for ( int i = 0; i < _row; i++)
		{
			for ( int j = 0; j < _column; j++) 
			{
				(*this)(i, j) = arg(i, j);
			}
		}
	}

	Matrix::Matrix(const Frame2& arg)
	{
		_row = 3;
		_column = 3;
		int length = _row * _column;
		_datas = new double[length];

		for ( int i = 0; i < _row; i++)
		{
			for ( int j = 0; j < _column; j++) 
			{
				(*this)(i, j) = arg(i, j);
			}
		}
	}

	Matrix::Matrix(const Frame3& arg)
	{
		_row = 4;
		_column = 4;
		int length = _row * _column;
		_datas = new double[length];

		for ( int i = 0; i < _row; i++)
		{
			for ( int j = 0; j < _column; j++) 
			{
				(*this)(i, j) = arg(i, j);
			}
		}
	}

	Matrix::Matrix(const Inertia& arg)
	{
		_row = 3;
		_column = 3;
		int length = _row * _column;
		_datas = new double[length];

		for ( int i = 0; i < _row; i++)
		{
			for ( int j = 0; j < _column; j++) 
			{
				(*this)(i, j) = arg(i, j);
			}
		}
	}

	Matrix::~Matrix(void)
	{
		delete[] _datas;
	}

	//Operator overloads.
	double Matrix::operator()(const int row, const int col) const
	{
		if((row < 0) || (row >= this->_row))
			throw(new std::exception("Argument row index must be >= 0 and < matrix'srow."));
		if((col < 0) || (col >= this->_column))
			throw(new std::exception("Argument col index must be >= 0 and < matrix'column."));

		return this->_datas[row*this->_column +col];
	}

	double& Matrix::operator() (const int row, const int col)
	{
		if((row < 0) || (row >= this->_row))
			throw(new std::exception("Argument row index must be >= 0 and < matrix'srow."));
		if((col < 0) || (col >= this->_column))
			throw(new std::exception("Argument col index must be >= 0 and < matrix'column."));

		return this->_datas[row*this->_column +col];
	}

	Matrix& Matrix::operator = ( const Matrix& arg)
	{
		if((_row == 0) && (_column == 0))
		{
			_row = arg._row;
			_column = arg._column;

			int length = _row * _column;
			_datas = new double[length];

			for(int i=0; i< length; i++)
				_datas[i] = arg._datas[i];
		}
		else
		{
			if((_row != arg._row) || (_column != arg._column))
				throw(new std::exception("Two matrix must have the same row and column count."));

			int length = _row * _column;
			for(int i=0; i< length; i++)
			{
				_datas[i] = arg._datas[i];
			}
		}

		return *this;
	}

	bool operator == (const Matrix& lhs, const Matrix& rhs)
	{
		return Equal(lhs,rhs);
	}

	bool operator != (const Matrix& lhs, const Matrix& rhs)
	{
		return !Equal(lhs,rhs);
	}

	Matrix operator + (const Matrix& lhs,const Matrix& rhs)
	{
		if((lhs._row != rhs._row) || (lhs._column != rhs._column))
			throw(new std::exception("Two matrix must have the same row and column count."));

		Matrix res = Matrix(lhs);
		int length = res._row * res._column;
		for(int i=0; i< length; i++)
		{
			res._datas[i] = res._datas[i] + rhs._datas[i];
		}
		return res;
	}

	Matrix operator - (const Matrix& lhs,const Matrix& rhs)
	{
		if((lhs._row != rhs._row) || (lhs._column != rhs._column))
			throw(new std::exception("Two matrix must have the same row and column count."));

		Matrix res = Matrix(lhs);
		int length = res._row * res._column;
		for(int i=0; i< length; i++)
		{
			res._datas[i] = res._datas[i] - rhs._datas[i];
		}
		return res;
	}

	Matrix operator * (const Matrix& lhs,const Matrix& rhs)
	{
		if(lhs._column != rhs._row)
			throw(new std::exception("Argument lhs's column count must be the same as rhs's row count."));

		Matrix res = Matrix(lhs._row, rhs._column);

		for ( int i = 0; i < res._row; ++i)
		{
			for ( int j = 0; j < res._column; ++j) 
			{
				double temp = 0;
				for ( int k = 0; k < lhs._column; ++k)
				{
					temp += lhs( i, k) * rhs( k, j);
				}

				res( i, j) = temp;
			}
		}
		return res;
	}

	Matrix operator * (const double lhs,const Matrix& rhs)
	{
		Matrix res = Matrix(rhs);
		int length = res._row * res._column;
		for(int i=0; i< length; i++)
		{
			res._datas[i] = res._datas[i] * lhs;
		}
		return res;
	}

	Matrix operator * (const Matrix& lhs,const double rhs)
	{
		Matrix res = Matrix(lhs);
		int length = res._row * res._column;
		for(int i=0; i< length; i++)
		{
			res._datas[i] = res._datas[i] * rhs;
		}
		return res;
	}

	ColumnVector operator * (const Matrix& lhs,const ColumnVector& rhs)
	{
		if(lhs._column != rhs.GetCount())
			throw(new std::exception("Argument lhs's column count must be the same as rhs's count."));

		ColumnVector res = ColumnVector(lhs._row);

		for ( int i = 0; i < lhs._row; i++)
		{
			double temp = 0;
			for ( int j = 0; j < lhs._column; j++) 
			{
				temp += lhs( i, j) * rhs[j];
			}
			res[i] = temp;
		}

		return res;
	}

	Matrix operator / (const Matrix& lhs,const double rhs)
	{
		Matrix res = Matrix(lhs);
		int length = res._row * res._column;
		for(int i=0; i< length; i++)
		{
			res._datas[i] = res._datas[i] / rhs;
		}
		return res;
	}

	//double DotProduct(const Matrix& lhs,const double rhs)
	//{
	//	if((lhs._row <= 1) != (lhs._column != 1))
	//		throw(new std::exception("lhs's row count must > 1 and column count must == 1."));
	//	if((rhs._row <= 1) != (rhs._column != 1))
	//		throw(new std::exception("rhs's row count must > 1 and column count must == 1."));

	//	double res = 0;
	//	for(int i = 0; i < lhs._row; i++)
	//		res += lhs._datas[i] * rhs._datas[i];
	//}

	////Cross product, 
	////Only works when lhs and rhs are column vector.
	////lhs and rhs's row count > 1, and column count ==1.
	////lhs and rhs's row count must be equal
	//friend Matrix CrossProduct(const Matrix& lhs,const double rhs)
	//{
	//	if((lhs._row <= 1) != (lhs._column != 1))
	//		throw(new std::exception("lhs's row count must > 1 and column count must == 1."));
	//	if((rhs._row <= 1) != (rhs._column != 1))
	//		throw(new std::exception("rhs's row count must > 1 and column count must == 1."));

	//	double res = 0;
	//	for(int i = 0; i < lhs._row; i++)
	//		res += lhs._datas[i] * rhs._datas[i];
	//}

	//Gets determine, this matrix' row and column must be equal.
	double Matrix::Determinant()
	{
		if(_column != _row)
			throw(new std::exception("Matrix must have the same row and column count."));
		if(_column <= 0)
			throw(new std::exception("Matrix's row and column count must > 0."));

		if(_column == 1)
			return (*this)(0, 0);
		else
		{
			double d = 0;
			for ( int i = 0; i < _row; ++i)
			{
				int sgn = ( i % 2) ? -1 : 1;
				Matrix cf = MatrixCofactor( i, 0); 
				d += sgn * (*this)(i, 0) * cf.Determinant();
			}
			return d;
		}
	}

	//Gets matrix's row count
	int Matrix::GetRow() const
	{
		return _row;
	}

	//Gets matrix's column count
	int Matrix::GetColumn() const
	{
		return _column;
	}

	Matrix Matrix::GetSubMatrix(const int startRow, const int endRow, const int startCol, const int endCol)
	{
		if(startRow < 0)
			throw(new std::exception("Argument startRow must >= 0."));
		if(endRow >= _row)
			throw(new std::exception("Argument endRow must < this matrix's row count."));
		if(startCol < 0)
			throw(new std::exception("Argument startCol must >= 0."));
		if(endCol >= _column)
			throw(new std::exception("Argument endCol must < this matrix's column count."));
		if(startRow > endRow)
			throw(new std::exception("Argument startRow must <= endRow."));
		if(startCol > endCol)
			throw(new std::exception("Argument startCol must <= endCol."));

		Matrix res = Matrix(1 + endRow - startRow, 1 + endCol - startCol);
		for ( int i = 0; i < res._row; ++i)
		{
			for ( int j = 0; j < res._column; ++j) 
			{
				res( i, j ) = (*this)(startRow + i, startCol + j);
			}
		}

		return res;
	}

	//Sets as identity matrix, this matrix' row and column must be equal.
	void Matrix::Identity()
	{
		if(_column != _row)
			throw(new std::exception("Matrix must have the same row and column count."));

		for ( int i = 0; i < _row; ++i)
		{
			for ( int j = 0; j < _column; ++j) 
			{
				if(i == j)
					(*this)( i, j) = 1;
				else
					(*this)( i, j) = 0;
			}
		}
	}

	Matrix Matrix::Inverse()
	{
		if(_column != _row)
			throw(new std::exception("Matrix must have the same row and column count."));
		double deter = this->Determinant();
		if(deter == 0)
			throw(new std::exception("Matrix's determinant must not be 0."));

		Matrix res = Matrix(_row, _column);
		for ( int i = 0; i < _column; ++i)
		{
			for ( int j = 0; j < _column; ++j) 
			{
				int sgn = ( (i+j)%2) ? -1 : 1;
				res( i, j) = sgn * MatrixCofactor( i,j).Determinant();
			}
		}
		res = res.Transpose();
		res =  res / deter;
		return res;
	}

	Matrix Matrix::MatrixCofactor(const int row, const int col)
	{
		if((row < 0) || (row >= this->_row))
			throw(new std::exception("Argument row index must be >= 0 and < matrix'srow."));
		if((col < 0) || (col >= this->_column))
			throw(new std::exception("Argument col index must be >= 0 and < matrix'column."));

		Matrix res = Matrix(_row - 1, _column - 1);
		for ( int i = 0, i2 = 0; i < _column; ++i)
		{
			if(i != row)
			{
				for ( int j = 0, j2 = 0; j < _column; ++j) 
				{
					if(j != col)
					{
						res( i2, j2) = (*this)(i, j);
						j2++;
					}
				}
				i2++;
			}
		}

		return res;

	}

	//Gets this matrix's trace.
	double Matrix::Trace()
	{
		if(_column != _row)
			throw(new std::exception("Matrix must have the same row and column count."));

		double res = 0;
		for(int i = 0; i < _row; i++)
			res = res + (*this)(i,i);

		return res;
	}

	Matrix Matrix::Transpose()
	{
		Matrix res = Matrix(_column, _row);
		for ( int i = 0; i < _row; i++)
		{
			for ( int j = 0; j < _column; j++)
			{
				res(j, i) = (*this)(i, j);
			}
		}

		return res;
	}

	void Matrix::SetZero()
	{
		for ( int i = 0; i < _row; i++)
		{
			for ( int j = 0; j < _column; j++)
			{
				(*this)(i, j) = 0;
			}
		}
	}

	bool Equal(const Matrix& lhs,const Matrix& rhs,const double eps)
	{
		if((lhs._column != rhs._column) || (lhs._row != rhs._row))
			throw(new std::exception("lhs's row and column number must be equal to rhs's row and column number."));

		for(int i = 0; i < lhs._row; i++)
		{
			for(int k = 0; k < lhs._column; k++)
			{
				if(Equal(lhs(i,k),rhs(i,k),eps) == false)
					return false;
			}
		}

		return true;

	}
}}